import rospy
import math
import time
from sensor_msgs.msg import LaserScan
#from geometry_msgs.msg import Twist
from std_msgs.msg import String
import os
import sys
path = os.path.abspath(".")
sys.path.insert(0,path + "/src/car_pkg/scripts")
import USART


sender = USART.SerialSender("/dev/ttyS4", 115200)
print("/dev/ttyS4;115200")
received_array=[1,1,1,1,1,1]
scan_distance = 0.0

def index(a,a_min,a_inc):
    return int(((a*math.pi)/180 - a_min)/a_inc)

def lidar_back(data):
    global scan_distance
    scan_filter = [ ]
    for i in range(index(170,data.angle_min,data.angle_increment),index(180,data.angle_min,data.angle_increment)):
        if data.ranges[i] > 0.1:
            scan_filter.append(data.ranges[i])
    for i in range(index(-180,data.angle_min,data.angle_increment),index(-170,data.angle_min,data.angle_increment)):
        if data.ranges[i] > 0.1:
            scan_filter.append(data.ranges[i])
    #rospy.loginfo("Distance :"+ str(min(scan_filter)))
    scan_distance = min(scan_filter)
    rospy.loginfo("distance_forward = %f m",scan_distance)


def main():
    rospy.init_node('laserscan_node',anonymous =False)
    rospy.Subscriber('/scan',LaserScan,lidar_back,queue_size =10)
    
    
    rospy.spin()

if __name__ == "__main__":
    main()
        
    
